#include <windows.h>
#include <stdio.h>
#define _VICONLIB_STATIC
#include "iViconHandler.h"
#include <tchar.h>
#include <strsafe.h>
#include <intrin.h>

//#include <fstream>
#include <math.h>
#include <iostream>
#include <fstream>
using namespace std;
unsigned char *magic;
filebuf fb;

#define BUFSIZE 4096 
 
char chBuf[BUFSIZE]; 
//vicon variables
iViconHandler* viconH;
ViconFrame frame;
double xcoord=0;
double ycoord=0;
double zcoord=0;

#include <sys/stat.h>

bool FileExists(string strFilename) {
  struct stat stFileInfo;
  bool blnReturn;
  int intStat;

  // Attempt to get the file attributes
  intStat = stat(strFilename.c_str(),&stFileInfo);
  if(intStat == 0) {
    // We were able to get the file attributes
    // so the file obviously exists.
    blnReturn = true;
  } else {
    // We were not able to get the file attributes.
    // This may mean that we don't have permission to
    // access the folder which contains this file. If you
    // need to do that level of checking, lookup the
    // return values of stat which will give you
    // more details on why stat failed.
    blnReturn = false;
  }
  
  return blnReturn;
}

int main(void) 
{ 
	//setup
	viconH = CreateViconHandler();
	if (!viconH)
	{
		return -1;
	}
	viconH->Initialise();

	viconH->TrackBody("Hat","Hat");

	//int x = viconH->Connect("127.0.0.1", 800);
	//int x = viconH->Connect("172.31.58.61", 800);
	int x = viconH->Connect("128.232.14.120", 800);
	if (x != RESULT_OK)
	{
		return -1;
	}

	bool newCoord = true;


   for (;;) 
   {
	   //check if we should get a new coordinate
	   if(newCoord==true)
	   {
		   //fetch new coordinate
			viconH->GetFrame(&frame);

			ViconBody* body = frame.GetBodyByName("Hat","Hat");
			// Write to standard output and stop on error.
			//char out [100] = "testoutput";
			if (body!=NULL)
			{
				if (body->GetIsDataValid())
				{
					Vector3 coords;
					coords=body->GetWorldPos();
					xcoord = coords.x;
					ycoord = coords.y;
					zcoord = coords.z;
				}
			}

			//write to file
			char out [BUFSIZE];
			wsprintfA(out,"%.15li\n%.15li\n%.15li\n\n",xcoord,ycoord,zcoord);
			char outputFilename[] = "coord.vic";
			fb.open(outputFilename, ios::out);
			ostream outFile(&fb);
			outFile << out;
			fb.close();

			//create lock file to indicate new coordinates
			char outputFilename2[] = "lock.vic";
			fb.open(outputFilename2, ios::out);
			ostream outFile2(&fb);
			outFile2 << "b";
			fb.close();
	   }

	   //sleep for a bit
	   Sleep (100);

	   //check if lock has been removed
	   newCoord = !FileExists("lock.vic");

	} 
	return 0;
}